#include "as5600.h"
#include "esp_log.h"
#include "esp_timer.h"
#include <math.h>

#define AS5600_ADDR       0x36
#define ANGLE_REG_MSB     0x0C
#define AS5600_RESOLUTION 4096
#define _2PI 6.28318530718f
#define RAD_TO_DEG  (180.0f / M_PI)

static const char *TAG = "AS5600";

// 读取单圈原始角度（0~2π）
float AS5600_getSensorAngle(AS5600_t *sensor)
{
    uint8_t data[2];
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();

    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (AS5600_ADDR << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write_byte(cmd, ANGLE_REG_MSB, true);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (AS5600_ADDR << 1) | I2C_MASTER_READ, true);
    i2c_master_read(cmd, data, 2, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);

    esp_err_t err = i2c_master_cmd_begin(sensor->i2c_num, cmd, 100 / portTICK_PERIOD_MS);
    i2c_cmd_link_delete(cmd);

    if (err != ESP_OK) {
        ESP_LOGW(TAG, "Read angle failed");
        return sensor->angle_prev; // 读取失败时返回上次角度，避免跳变
    }

    uint16_t raw = ((data[0] << 8) | data[1]) & 0x0FFF; // 12-bit 数据
    return ((float)raw / AS5600_RESOLUTION) * _2PI;
}

// 初始化传感器结构体和I2C
void AS5600_init(AS5600_t *sensor, i2c_port_t i2c_num, gpio_num_t sda_io, gpio_num_t scl_io)
{
    sensor->i2c_num = i2c_num;
    sensor->sda_io = sda_io;
    sensor->scl_io = scl_io;

    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = sda_io,
        .scl_io_num = scl_io,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 400000
    };
    ESP_ERROR_CHECK(i2c_param_config(i2c_num, &conf));
    ESP_ERROR_CHECK(i2c_driver_install(i2c_num, I2C_MODE_MASTER, 0, 0, 0));

    sensor->full_rotations = 0;
    sensor->vel_full_rotations = 0;

    sensor->vel_angle_prev = AS5600_getSensorAngle(sensor);
    sensor->vel_angle_prev_ts = esp_timer_get_time();

    sensor->angle_prev = sensor->vel_angle_prev;
    sensor->angle_prev_ts = sensor->vel_angle_prev_ts;
}

// 更新角度及圈数，需周期调用
void AS5600_update(AS5600_t *sensor)
{
    float val = AS5600_getSensorAngle(sensor);
    int64_t now = esp_timer_get_time();

    // 反向计算角度差，修正方向
    float d_angle = sensor->angle_prev - val;

    // 跨圈检测（超过0.8圈的跳变，视为圈数变化）
    if (fabsf(d_angle) > (0.8f * _2PI)) {
        sensor->full_rotations += (d_angle > 0) ? 1 : -1;
        // ESP_LOGI(TAG, "Cross circle detected: d_angle=%.3f, full_rotations=%d", d_angle, sensor->full_rotations);
    }

    sensor->angle_prev = val;
    sensor->angle_prev_ts = now;
}

// 获取当前机械角度（单圈，单位：弧度）
float AS5600_getMechanicalAngle(AS5600_t *sensor)
{
    return sensor->angle_prev;
}

// 获取多圈角度（单位：弧度，包含圈数）
float AS5600_getAngle(AS5600_t *sensor)
{
    return (float)sensor->full_rotations * _2PI + sensor->angle_prev;
}

// 获取当前角速度（单位：弧度/秒）
float AS5600_getVelocity(AS5600_t *sensor)
{
    int64_t now = esp_timer_get_time();
    float Ts = (now - sensor->vel_angle_prev_ts) * 1e-6f;
    if (Ts <= 0) Ts = 1e-3f;

    float delta = ((sensor->full_rotations - sensor->vel_full_rotations) * _2PI)
                  + (sensor->angle_prev - sensor->vel_angle_prev);
    float vel = delta / Ts;

    // 更新速度计算状态变量
    sensor->vel_angle_prev = sensor->angle_prev;
    sensor->vel_full_rotations = sensor->full_rotations;
    sensor->vel_angle_prev_ts = now;

    return vel;
}
